/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: cameraCtrl_thread.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <time.h>
#include <stdio.h>

#include "globaldefs/globaldefs.h"
#include "utilities/utilities.h"

void *cameraCtrl()
{
	
	struct timespec t0,t1,t0_ant,wait,next_activation,WCCT,Period;
	_uint64 auxtime;
	clock_gettime(CLOCK_REALTIME, &t0_ant);
	printf ("Camera Control Task on-line.\n");	
	while(1) 
	{
		
		// Loop time claculations
		clock_gettime (CLOCK_REALTIME, &t0);
		next_activation=sum_tiempos(t0,Ts_cameraCtrl); 
		WCCT = diff_tiempos(ID_cameraCtrl,t0_ant,t1,0);
		
		// Print the results
		//if(v) printf("Inicialization--> WCCT: %3d Period: %3d \n",((int)WCCT.tv_nsec)/prec,  ((int)t0.tv_nsec-(int)t0_ant.tv_nsec)/prec);
		if(v) printf("C %3d %3d \n",((int)WCCT.tv_nsec)/prec,  ((int)Period.tv_nsec)/prec);
		// Wait until the next activation
		Period=diff_tiempos(ID_cameraCtrl,t0,t0_ant,0);
		t0_ant=t0;
		clock_gettime (CLOCK_REALTIME, &t1);
		if (timespec2nsec(&next_activation)>timespec2nsec(&t1))
		{
		    auxtime = (timespec2nsec(&next_activation)-timespec2nsec(&t1));
			nsec2timespec(&wait,auxtime);
			nanosleep(&wait, NULL);
		}
	}
	return(NULL);
}